package lte

import (
	"fmt"
	"math"
	"math/cmplx"
	"time"

	"gitee.com/thubcc/mobile/utils"
	"github.com/prometheus/common/log"
)

const (
	sInit = iota
	sPss
	sScan
	sTiming
	sCarrier
)

const (
	Interval = 20
)

type AFC struct {
	state    int
	reset    bool
	ftco     utils.RFInterface
	bestid   int
	pos      int
	duration int
	rs       *PeakRecords
	tpid     PID
	fpid     PID
	ar       afcRecord
}

type PLL struct {
	A0 float64 `json:"a0"`
	A1 float64 `json:"a1"`
}

func NewAFC(ftco utils.RFInterface, reset bool, rs int) *AFC {
	return &AFC{
		state:    sInit,
		reset:    reset,
		ftco:     ftco,
		bestid:   0,
		pos:      0,
		duration: 5,
		rs:       NewPeakRecords(rs),
		tpid:     PID{1e-3, 1e-4, 0.0},
		fpid:     PID{1.0, 0.1, 0.0},
		ar:       afcRecord{fft: utils.NewFFT(1024)},
	}
}

func (a *AFC) ChangeCid(cid int) {
	a.bestid = cid
}

func (a *AFC) Init() {
	if a.reset {
		a.ftco.ResetPPM()
	} else {
		a.ftco.InitPPM()
	}
	a.ftco.HardReset()
	a.state = sPss
}

func (a *AFC) Pss() {
	for pssc := 0; pssc < 5; pssc++ {
		a.ftco.Reset()
		rfd := a.ftco.Raw(0, 100)
		a.ftco.Run(100)
		df, _, t, pos := ScanPssAll.SearchPSS(rfd, 874.2e6)
		// log.Info("Searching ", df, amp, t, pos)
		if math.Abs(df) < 3e3 {
			// log.Info("AFC Pss ready")
			a.state = sScan
			a.pos = pos
			a.bestid = t
			return
		} else {
			// log.Info("Set df:", df)
			a.ftco.PPMF(df, false)
		}
	}
	a.state = sInit
}

func (a *AFC) Scan() {
	a.ftco.Reset()
	rfd := a.ftco.Raw(0, 2)
	a.ftco.Run(2)
	rs := ScanRefSym(rfd, a.bestid%3, 0, a.pos)
	df := rs[0].Df
	// log.Info("Scan all ref signal ", rs[0].Amp, rs[0].Cid, rs[0].Pos, rs[0].Df)
	a.bestid, a.pos = rs[0].Cid, rs[0].Pos
	// log.Info("Set df:", df)
	a.ftco.PPMF(df, false)
	a.state = sTiming
	a.duration = 1
}

type TimingResult struct {
	Df   float64 `json:"freqoff"`
	Mean float64 `json:"pos"`
	Std  float64 `json:"std"`
	Amp  float64 `json:"amp"`
}

func (a *AFC) CarrierOne(timingFirst bool) (df float64) {
	rfd := a.ftco.Raw(0, 100)
	pos, _ := RefSignalPeak(rfd, a.bestid, 0)
	a.ftco.Run(100)
	df = AFCSoltofCell(rfd, a.bestid, 0, pos)
	if math.Abs(df) < 50 {
		a.ftco.SetDf(df / 2.)
		a.ftco.Reset()
	}
	log.Info("df: ", df)
	return
}

func (a *AFC) TimingOne(rfd []complex128, cid int, rs *PeakRecords) (res TimingResult, ok bool) {
	pos, amp := RefSignalPeak(rfd, cid, 0)
	dt := timing(rfd, cid, pos)
	res.Df, res.Mean, res.Std, ok = rs.Add(NewPeakRecord(dt))
	res.Amp = amp
	return
}

func (a *AFC) AdjustPPMT(tr *TimingResult) (ok bool) {
	if math.Abs(tr.Std) < 100 && math.Abs(tr.Df) < 5 {
		a.ftco.SetDt(-tr.Df / 2)
		a.ftco.Reset()
		return true
	} else {
		return false
	}
}
func (a *AFC) NeedReset(tr *TimingResult) bool {
	if tr.Std > 100 {
		a.state = sInit
		return true
	} else {
		return false
	}
}

func (a *AFC) AdjustHealth(tr *TimingResult) {
	health := 10.0 - math.Log2(tr.Std)
	if health < 5 {
		health = 5.
	} else if health > 30 {
		health = 30.
	}
	a.duration = int(health)
}

func (a *AFC) AdjustFrameStart(tr *TimingResult) {
	if math.Abs(tr.Mean-9600) > 0.5 {
		log.Info("Reset frame start ", tr.Mean)
		a.ftco.FrameStartAdjust(int64(9600 + tr.Mean + 0.5))
		a.ftco.HardReset()
		a.ftco.Reset()
	}
}

func (a *AFC) Timing() (res TimingResult, ok bool) {
	var rfd []complex128
	for i := 0; i < a.duration; i++ {
		rfd = a.ftco.Raw(0, 4)
		a.ftco.Run(Interval)
	}
	res, ok = a.TimingOne(rfd, a.bestid, a.rs)
	if ok {
		if a.NeedReset(&res) {
			return
		}
		a.AdjustHealth(&res)
		a.AdjustFrameStart(&res)
	}
	return
}

func timing(rfd []complex128, cid, peak int) float64 {
	l := len(rfd)
	H := HofCell(rfd[l-2*19200:], cid, 0, peak)
	sx := 0.
	sxx := 0.
	for i, v := range H[54:74] {
		a := cmplx.Abs(v)
		sx += float64(i) * a * a
		sxx += a * a
	}
	idx := sx / sxx
	r := idx + float64(peak-10)
	return r
}

func (a *AFC) AFCThreading() {
	for true {
		switch a.state {
		case sInit:
			a.Init()
		case sPss:
			a.Pss()
		case sScan:
			a.Scan()
		case sTiming:
			a.Timing()
		default:
			a.state = sInit
		}
	}
}

type PowerWeight struct {
	p, w float64
}

func (p *PowerWeight) add(i int, c complex128) {
	pp := c
	ppr := real(pp)*real(pp) + imag(pp)*imag(pp)
	p.w += float64(i) * ppr
	p.p += ppr
}

func (p *PowerWeight) weight() float64 {
	return p.w / p.p
}

func (a *AFC) errors(tdch []complex128) (te float64, fe complex128) {

	pw := PowerWeight{0.0, 0.0}
	for i, v := range tdch[128-16:] {
		pw.add(i-16, v)
	}
	for i, v := range tdch[:16] {
		pw.add(i, v)
	}
	te = pw.weight()
	mid := make([]complex128, 5)
	for i := range mid {
		p := int(te + float64(i) + .5)
		if p < 0 {
			mid[i] = tdch[128+p]
		} else {
			mid[i] = tdch[p]
		}
	}
	best := mid[0]
	abest := cmplx.Abs(best)
	for _, v := range mid[1:] {
		if cmplx.Abs(v) > abest {
			abest = cmplx.Abs(v)
			best = v
		}
	}
	fe = best
	return
}

type PID struct {
	a0, a1 float64
	sum    float64
}

func (p *PID) update(e float64) float64 {
	p.sum += e
	return p.a0*e + p.a1*p.sum
}

func (p *PID) reset(pll PLL) {
	p.a0 = pll.A0
	p.a1 = pll.A1
	p.sum = 0.
}

func (p *PID) GetSum() float64 {
	return p.sum
}

type afcRecord struct {
	buf   []complex128
	start time.Time
	fft   *utils.FFTPlan
}

func (a *afcRecord) reset() {
	a.buf = []complex128{}
	a.start = time.Now()
}

func (a *afcRecord) calculate() (df float64, err error) {
	end := time.Now()
	duration := end.Sub(a.start)
	if len(a.buf) < 32 {
		err = fmt.Errorf("too short ", len(a.buf))
		return
	}
	fft := a.fft
	n := len(fft.Buf)
	df = float64(len(a.buf)) / duration.Seconds() / float64(n)
	for i := range fft.Buf {
		fft.Buf[i] = 0.0 + 0.0i
	}
	copy(fft.Buf, a.buf)
	fft.Execute()
	pidx, pamp := utils.CAbsMax1D(fft.Buf[:n/2])
	nidx, namp := utils.CAbsMax1D(fft.Buf[n/2:])
	idx := 0
	if pamp > namp {
		idx = pidx
	} else {
		idx = nidx - n/2
	}
	df *= float64(idx)
	a.reset()
	return
}

func (a *afcRecord) add(pe complex128) {
	a.buf = append(a.buf, pe)
}

func (a *AFC) Trace() (tf float64) {
	var rfd []complex128
	rfd = a.ftco.Raw(0, 2)
	frame := rfd[9600 : 9600+19200]
	symbols := ofdm_rx(frame, 6)
	ch := ChannelEstimateDL(symbols, a.bestid, 0)
	tdch := ch.GetCh()
	te, fe := a.errors(tdch)
	tf = a.tpid.update(te)
	a.ar.add(fe)
	return
}

func (a *AFC) Reset(t, f PLL) {
	a.tpid.reset(t)
	a.fpid.reset(f)
	a.ar.reset()
}

func (a *AFC) FreqOffset() float64 {
	df, err := a.ar.calculate()
	if err != nil {
		log.Info("Failed to calculate ", err)
		df = 0.0
	}
	return df
}
func (a *AFC) Peak() (pos int) {
	rfd := a.ftco.Raw(0, 2)
	pos, _ = RefSignalPeak(rfd, a.bestid, 0)
	return
}
func (a *AFC) TPLLState() float64 {
	return a.tpid.GetSum()
}
